#include "AIP_goto.h"
#include "../../game_engine.h"

#include <cmath>

AIP_goto::AIP_goto(clock_t now,float spd,fcoord orig,fcoord dest):
AI_package(), initialTime( now ), speed( spd ), origin( orig ), destination( dest ),
pth( map::round(orig),  map::round(dest) ){
    type = AIPT_goto;
    if( !pth.compute(game_engine::getMap()) ){
        status = AIS_interrupted;
        interruption_flag = AIPI_path_not_found;
        return;
    }
    uicoord stack;
    if( pth.getNextPoint(&stack) ){
        destination = map::toFloat(stack);
        float dx = destination.x - origin.x, dy = destination.y - origin.y;
        duration = (sqrt(dx*dx + dy*dy)/speed) * 60 * CLOCKS_PER_SEC;
    }
    else{
        status = AIS_finished;
        printf("Finished\n");}
}

bool AIP_goto::recalculate(map* mymap,clock_t now,fcoord curpos){
    uicoord stpt = map::round(origin);
    if( !pth.compute(mymap, &stpt) ) return false;
    status = AIS_running;
    interruption_flag = AIPI_none;
    if( pth.getNextPoint( &stpt )){
        destination = map::toFloat(stpt);
        origin = curpos;
        float dx = destination.x - origin.x, dy = destination.y - origin.y;
        duration = (sqrt(dx*dx + dy*dy)/speed) * 60 * CLOCKS_PER_SEC;
        initialTime = now;
    }
    else{
        status = AIS_finished;
        printf("Finished\n");}
    return true;
}

void AIP_goto::AI_exec(character* ch, clock_t now){

    if( status != AIS_running ) return;

    float curpos = float(now - initialTime)/duration;
    if( curpos >= 1 ){
        ch->coord = destination;
        origin = destination;
        uicoord cstack;
        if( pth.getNextPoint(&cstack) ){
            initialTime = now;
            destination = map::toFloat(cstack);
            float dx = destination.x - origin.x, dy = destination.y - origin.y;
            duration = (sqrt(dx*dx + dy*dy)/speed) * 60 * CLOCKS_PER_SEC;
        }
        else{
            status = AIS_finished;
            printf("Finished\n");}
    }

}

void AIP_goto::char_update(character* ch,clock_t now){

    if( status != AIS_running ) return;

    float curpos = float(now - initialTime)/duration;
    if( curpos >= 1 ){
        ch->coord = destination;
        origin = destination;
        uicoord cstack;
        if( pth.getNextPoint(&cstack) ){
            initialTime = now;
            destination = map::toFloat(cstack);
            float dx = destination.x - origin.x, dy = destination.y - origin.y;
            duration = (sqrt(dx*dx + dy*dy)/speed) * 60 * CLOCKS_PER_SEC;
        }
        else{
            status = AIS_finished;
            printf("Finished\n");}
        return;
    }

    fcoord position;
    position.x = origin.x * (1 - curpos) + destination.x*curpos;
    position.y = origin.y * (1 - curpos) + destination.y*curpos;

    uicoord rd_pos = map::round(position);
    map_elem* el = game_engine::getMap()->elem(rd_pos);
    if( !map::isWalkable( game_engine::getMap()->elem(rd_pos) ) ){
        status = AIS_interrupted;
        interruption_flag = AIPI_blocked;
        return;
    }

    ch->coord = position;

}

float AIP_goto::getProgress(character* ch, clock_t now){
    return float(now - initialTime)/float(duration);
}

